
#include "CoordSys.h"
#include "literal.h"

CoordSys::CoordSys()
{

}

// 构造函数：初始化椭球参数并计算e2
CoordSys::CoordSys(const RefEllip& ellip1) : ellip(ellip1) {
    // 计算第一偏心率平方
    e2 = 2 * ellip1.f - ellip1.f * ellip1.f;
    this->ellip = ellip1;
}

// BLH转XYZ
XYZ CoordSys::BLH2XYZ(const BLH& blh) const {
    double B_rad = blh.B * D2R;
    double L_rad = blh.L * D2R;

    double sinB = sin(B_rad);
    double N = ellip.a / sqrt(1 - e2 * sinB * sinB);  // 卯酉圈曲率半径

    double cosB = cos(B_rad);
    double cosL = cos(L_rad);
    double sinL = sin(L_rad);

    return {
        (N + blh.H) * cosB * cosL,
        (N + blh.H) * cosB * sinL,
        (N * (1 - e2) + blh.H) * sinB
    };
}

// XYZ转BLH (迭代法)
BLH CoordSys::XYZ2BLH(const XYZ& xyz) const {
    double R0 = sqrt(xyz.x * xyz.x + xyz.y * xyz.y);
    double R1 = sqrt(R0 * R0 + xyz.z * xyz.z);

    double B = atan2(xyz.z, R0);
    double H = R1 - ellip.a;
    double prevB, prevH;
    const double eps = 1e-9;  // 收敛阈值

    do {
        prevB = B;
        prevH = H;

        double sinB = sin(B);
        double N = ellip.a / sqrt(1 - e2 * sinB * sinB);
        H = R0 / cos(B) - N;
        B = atan2(xyz.z, R0 * (1 - e2 * N / (N + H)));
    } while (fabs(B - prevB) > eps || fabs(H - prevH) > 1e-3);

    return { B / D2R, atan2(xyz.y, xyz.x) / D2R, H };
}

// XYZ转ENU
ENU CoordSys::XYZ2ENU(const XYZ& origin, const XYZ& point) const {
    BLH blh_origin = XYZ2BLH(origin);
    double B_rad = blh_origin.B * D2R;
    double L_rad = blh_origin.L * D2R;

    XYZ diff = subtract(point, origin);

    double sinB = sin(B_rad);
    double cosB = cos(B_rad);
    double sinL = sin(L_rad);
    double cosL = cos(L_rad);

    return {
        -sinL * diff.x + cosL * diff.y,
        -sinB * cosL * diff.x - sinB * sinL * diff.y + cosB * diff.z,
        cosB * cosL * diff.x + cosB * sinL * diff.y + sinB * diff.z
    };
}

// XYZ坐标减法
XYZ CoordSys::subtract(const XYZ& a, const XYZ& b) const {
    return { a.x - b.x, a.y - b.y, a.z - b.z };
}

// XYZ坐标求模
double CoordSys::mod(const XYZ& xyz) const {
    return sqrt(xyz.x * xyz.x + xyz.y * xyz.y + xyz.z * xyz.z);
}

void CoordSys::test()
{
    /* 测试坐标系统类 */
    BLH coord1 = { 34.808610734, 113.357448942, 120.2032 };

    // BLH -> XYZ 转换
    XYZ xyz1 = this->BLH2XYZ(coord1);
    cout << "坐标值1 BLH -> XYZ:\n"
        << fixed << setprecision(6)
        << "X: " << xyz1.x << " m\n"
        << "Y: " << xyz1.y << " m\n"
        << "Z: " << xyz1.z << " m\n\n";

    // XYZ -> BLH 转换
    BLH blh1_restore = this->XYZ2BLH(xyz1);
    cout << "坐标值1 XYZ -> BLH:\n"
        << "B: " << setprecision(9) << blh1_restore.B << " °\n"
        << "L: " << blh1_restore.L << " °\n"
        << "H: " << setprecision(4) << blh1_restore.H << " m\n\n";

    // 测试坐标值2
    BLH coord2 = { 34.807085734, 113.357000750, 120.8203 };
    XYZ xyz2 = this->BLH2XYZ(coord2);

    // 坐标差值和模计算
    XYZ diff = this->subtract(xyz2, xyz1);
    double mod = this->mod(diff);
    cout << "坐标值2 - 坐标值1:\n"
        << "ΔX: " << diff.x << " m\n"
        << "ΔY: " << diff.y << " m\n"
        << "ΔZ: " << diff.z << " m\n"
        << "模长: " << mod << " m\n\n";

    // 在坐标值1处展开ENU
    ENU enu = this->XYZ2ENU(xyz1, xyz2);
    cout << "在坐标值1处的ENU坐标:\n"
        << "东(E): " << enu.e << " m\n"
        << "北(N): " << enu.n << " m\n"
        << "天(U): " << enu.u << " m\n";
}


// BDCS派生类构造函数
BDCS::BDCS()
{
}
//BDCS::BDCS():CoordSys(BDSConsts().get_BDCSrefEllip()) {}
BDCS::BDCS(RefEllip ellip1)
{
    this->ellip = ellip1;
    this->e2 = 2 * ellip.f - ellip.f * ellip.f;

}
// WGS84派生类构造函数

WGS84::WGS84()
{
}

WGS84::WGS84(RefEllip ellip1)
{
    this->ellip = ellip1;
    this->e2 = 2 * ellip.f - ellip.f * ellip.f;
}